//
// Created by YangShuai on 2022/12/11.
//

/**
  ******************************************************************************
  * @file    driver_remote.c
  * @author  XJTU ROBOMASTER Team
  * @brief   remote driver
  *          这个文件提供函数对遥控器数据进行处理得到拨杆和键鼠控制量，对机器人控制信号进行更新
  *
  @verbatim
  ==============================================================================
                        ##### How to use this driver #####
  ==============================================================================
    [..]
      (#) Update the remote data by implementing the RC_Update():
          (++) Update the remote parameters, including:
		       (+++) RC_Ctl

      (#) Update the remote data by implementing the RemoteControl():
          (++) Update the remote parameters, including:
		       (+++) RemoteData

  @endverbatim
  ******************************************************************************
  * @attention
  *
  * 函数 RC_Update() 已经封装好，遥杆和键鼠的数据接收均为正确的不需要改
  * 可以根据需要更改结构体 RemoteDataPortStruct 中的成员变量，并更改函数 RemoteControl()
  *
  * Please add comments after adding or deleting functions to ensure code
  * specification.
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "driver_remote.h"
#include "math.h"
/* ----------------------- Internal Data ----------------------------------- */
/** @brief The data received from the Dbus, is the memory address of the serial
  *        port DMA transfer
  */
uint8_t remote_Rx_Buffer[RC_FRAME_LENGTH];//原始数据位置
float test_current;
uint8_t AutoSmallBuffShot;
uint8_t AutoBigBuffShot;
/** @brief According to the DT7 & DR16 2.4Ghz manual, 18 bytes of data are computed and
  *        stored in the struct variable
  */
//RC_Ctl_t RC_Ctl;
RemoteDataUnion RemoteData;//原始数据结构体
RemoteDataPortStruct RemoteDataPort;
enum
{
    REMOTE_MODE=1,
    KEYBOARD_MODE=2,
    AUTO_MODE=3,
}RemoteControlMode;


/** @brief All variables related to robot control
  */

RemoteControlFlagSetPortStruct RemoteControlFlagSetPort = {0};//特殊需求标志位
/* ----------------------- Function Implements ---------------------------- */
RemoteDataPortStruct RemoteModeProcessData(RemoteDataProcessedStruct	RemoteDataReceive)
{
    static RemoteDataPortStruct	RemoteDataPortTemp={0};

    RemoteDataPortTemp.ChassisSpeedX	=	-	RemoteDataReceive.Channel_2;
    RemoteDataPortTemp.ChassisSpeedY	=	-	RemoteDataReceive.Channel_3;

    RemoteDataPortTemp.PitchIncrement	=		RemoteDataReceive.Channel_1/16;
    RemoteDataPortTemp.YawIncrement		=	-	RemoteDataReceive.Channel_0/16;

    RockerDataConvert(&(RemoteDataPortTemp.ChassisSpeedX),&(RemoteDataPortTemp.ChassisSpeedY));

    //AimAutoData.AutoAim = 1;

    switch(RemoteDataReceive.RightSwitch)
    {
        case 1:RemoteDataPortTemp.Friction=DISABLE;
            RemoteDataPortTemp.FeedMotor=DISABLE;
            RemoteDataPortTemp.SuperCFlag=0;
            break;
        case 3:RemoteDataPortTemp.Friction=ENABLE;
            RemoteDataPortTemp.FeedMotor=DISABLE;
            RemoteDataPortTemp.SuperCFlag=1;
            break;
        case 2:RemoteDataPortTemp.Friction=ENABLE;
            RemoteDataPortTemp.FeedMotor=ENABLE;
            RemoteDataPortTemp.SuperCFlag=0;
            break;
        default:
            break;
    }
    RemoteDataPortTemp.Laser=RemoteDataPortTemp.Friction;

    return RemoteDataPortTemp;
}

RemoteDataPortStruct RemoteDataCalculate(RemoteDataProcessedStruct	RemoteDataReceive)
{
    RemoteDataPortStruct	RemoteDataPortTemp;

    RemoteControlMode	=	RemoteDataReceive.LeftSwitch;

    switch(RemoteControlMode)
    {
        case	REMOTE_MODE://1
            RemoteDataPortTemp	=	RemoteModeProcessData(RemoteDataReceive);
            RemoteDataPortTemp.SpinFlag         =   1;
            RemoteDataPortTemp.CancelFollow     =   RemoteControlFlagSetPort.CancelFollow;
            RemoteDataPortTemp.Magazine         =   RemoteControlFlagSetPort.Magazine;
            RemoteDataPortTemp.YawLocationInit  =   RemoteControlFlagSetPort.YawLocationInit;
            RemoteDataPortTemp.ChassisInitFlag  =   RemoteControlFlagSetPort.ChassisInitFlag;
            RemoteDataPortTemp.RampFlag         =   RemoteControlFlagSetPort.RampFlag;
            break;
        case	KEYBOARD_MODE://2
            break;
        case	AUTO_MODE://3
            RemoteDataPortTemp	=	RemoteModeProcessData(RemoteDataReceive);
            RemoteDataPortTemp.SpinFlag         =   0;
            RemoteDataPortTemp.CancelFollow     =   RemoteControlFlagSetPort.CancelFollow;
            RemoteDataPortTemp.Magazine         =   RemoteControlFlagSetPort.Magazine;
            RemoteDataPortTemp.YawLocationInit  =   RemoteControlFlagSetPort.YawLocationInit;
            RemoteDataPortTemp.ChassisInitFlag  =   RemoteControlFlagSetPort.ChassisInitFlag;
            RemoteDataPortTemp.RampFlag         =   RemoteControlFlagSetPort.RampFlag;

            break;
    }
    return RemoteDataPortTemp;
}



/**
  * @brief  Solve the DR16 Remote data
  * @param  remote_Rx_Buffer : original received data
  * @param  RC_Ctl : processed data
  * @retval None
  * 测试内容：首先检查RemoteData.RemoteDataRaw[i]中是否是DR16的原始数据，是否会根据遥控器的波动而改变
  * 当检查完数据之后，便检查RemoteData联合体中的摇杆及键盘数据是否正确。是否能够按照键鼠的改变而改变
  */
int RC_Update(void) {
    RemoteDataProcessedStruct RemoteDataReceive;        //建立虚拟控制器
    //Step	1	:	Receive remote raw data from buffer
    for (int i=0;i<18;i++)
        RemoteData.RemoteDataRaw[i] = remote_Rx_Buffer[i];//遥控器内容输入
    RemoteDataReceive = RemoteDataProcess(RemoteData);//检查数据正确并压缩到虚拟控制器中

    //Step	2	:	Judge Validity
    if (RemoteDataReceive.FlagValidity) {

        //Step	3	Process	remote data	and	Save into RemoteDataPort
        RemoteDataPort = RemoteDataCalculate(RemoteDataReceive);//重点：进行主要控制
        return 0;
    }
    return 1;
}
/**
  * @brief  Get all the control signals needed for robot control
  * @param  RC_Ctl : data before processing
  * @param  RemoteData : the processed data
  * @retval None
  * 这个函数的作用是做整个遥控器数据的控制与分发 并不参与到具体的控制中。所有的控制任务从这个结构体中取出数据并在自身文件中进行处理。
  */
//0,正常
//1，数据有误，不要用
uint8_t  JudgeDataValidity(uint16_t  data)
{
    if(data<=1685&&data>=363)
        return 0;
    else
        return 1;
}
RemoteDataProcessedStruct RemoteDataProcess(RemoteDataUnion RemoteDataRaw)
{
    RemoteDataProcessedStruct RemoteDataProcessed;


    uint8_t JudgeDataValiditySum=0;
    float MouseX,MouseY,MouseZ;
//**********************************摇杆***********************************************

    JudgeDataValiditySum = JudgeDataValidity(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch0) + JudgeDataValidity(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch1) + JudgeDataValidity(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch1) +JudgeDataValidity(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch3);

    if(JudgeDataValiditySum)					//检查数据合法性
    {
        RemoteDataProcessed.FlagValidity=0;
        return RemoteDataProcessed;
    }

    //基本量偏移消除
    if(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch0>1030||RemoteDataRaw.RemoteDataProcessed.RCValue.Ch0<1018)
        RemoteDataProcessed.Channel_0 =  ((float)(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch0-RC_CH_VALUE_OFFSET))/(RC_CH_VALUE_MAX-RC_CH_VALUE_OFFSET);
    else
        RemoteDataProcessed.Channel_0 = 0;

    //基本量偏移消除
    if(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch1>1030||RemoteDataRaw.RemoteDataProcessed.RCValue.Ch1<1018)
        RemoteDataProcessed.Channel_1 =  ((float)(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch1-RC_CH_VALUE_OFFSET))/(RC_CH_VALUE_MAX-RC_CH_VALUE_OFFSET);
    else
        RemoteDataProcessed.Channel_1 = 0;

    if(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch2>1030||RemoteDataRaw.RemoteDataProcessed.RCValue.Ch2<1018)
        RemoteDataProcessed.Channel_2 =  ((float )(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch2-RC_CH_VALUE_OFFSET))/(RC_CH_VALUE_MAX-RC_CH_VALUE_OFFSET);
    else
        RemoteDataProcessed.Channel_2 = 0;

    if(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch3>1030||RemoteDataRaw.RemoteDataProcessed.RCValue.Ch3<1018)
        RemoteDataProcessed.Channel_3 =  ((float)(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch3-RC_CH_VALUE_OFFSET))/(RC_CH_VALUE_MAX-RC_CH_VALUE_OFFSET);
    else
        RemoteDataProcessed.Channel_3 = 0;

//**********************************拨扭***********************************************
    RemoteDataProcessed.LeftSwitch = RemoteDataRaw.RemoteDataProcessed.RCValue.s2;
    RemoteDataProcessed.RightSwitch= RemoteDataRaw.RemoteDataProcessed.RCValue.s1;

//**********************************鼠标***********************************************
    MouseX = RemoteDataRaw.RemoteDataProcessed.Mouse.x/32768.0f;
    MouseY = RemoteDataRaw.RemoteDataProcessed.Mouse.y/32768.0f;
    MouseZ = RemoteDataRaw.RemoteDataProcessed.Mouse.z/32768.0f;

    if(MouseX>=MOUSEERRORTOLERANCE||MouseX<=-MOUSEERRORTOLERANCE)
        RemoteDataProcessed.MouseX	=	MouseX;
    else
        RemoteDataProcessed.MouseX	=	0;

    if(MouseY>=MOUSEERRORTOLERANCE||MouseY<=-MOUSEERRORTOLERANCE)
        RemoteDataProcessed.MouseY	=	MouseY;
    else
        RemoteDataProcessed.MouseY	=	0;

    if(MouseZ>=MOUSEERRORTOLERANCE||MouseZ<=-MOUSEERRORTOLERANCE)
        RemoteDataProcessed.MouseZ	=	MouseZ;
    else
        RemoteDataProcessed.MouseZ	=	0;

    RemoteDataProcessed.LeftMousePress	=	RemoteDataRaw.RemoteDataProcessed.Mouse.Press_l;
    RemoteDataProcessed.RightMousePress	=	RemoteDataRaw.RemoteDataProcessed.Mouse.Press_r;

    RemoteDataProcessed.KeyW	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyW;
    RemoteDataProcessed.KeyS	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyS;
    RemoteDataProcessed.KeyA	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyA;
    RemoteDataProcessed.KeyD	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyD;
    RemoteDataProcessed.KeyShift	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyShift;
    RemoteDataProcessed.KeyCtrl	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyCtrl;
    RemoteDataProcessed.KeyQ	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyQ;
    RemoteDataProcessed.KeyE	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyE;
    RemoteDataProcessed.KeyR	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyR;
    RemoteDataProcessed.KeyF	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyF;
    RemoteDataProcessed.KeyG	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyG;
    RemoteDataProcessed.KeyZ	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyZ;
    RemoteDataProcessed.KeyX	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyX;
    RemoteDataProcessed.KeyC	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyC;
    RemoteDataProcessed.KeyV	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyV;
    RemoteDataProcessed.KeyB	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyB;

    RemoteDataProcessed.FlagValidity=1;
    return RemoteDataProcessed;
}

void RockerDataConvert(float *x,float *y)
{
    if(fabs(*x)+fabs(*y)>1)
    {
        if(*y>0 && *x>0)				//1象限
        {
            *y=*y/(*y+*x);
            *x=*x/(*y+*x);
        }
        else if(*y>0 && *x<0)		//2象限
        {
            *x=*x/(*y-*x);
            *y=*y/(*y-*x);
        }
        else if(*y<0 && *x>0)		//4象限
        {
            *x=-*x/(*y-*x);
            *y=-*y/(*y-*x);
        }
        else										//3象限
        {
            *x=-*x/(*x+*y);
            *y=-*y/(*x+*y);
        }
    }
}

/************************ (C) COPYRIGHT XJTU ROBOMASTER ********END OF FILE****/
